from time import time, sleep
import socket
import pickle
from threading import Thread
import pos_nokov as pos
from concurrent.futures import ThreadPoolExecutor
from position import *
import copy

count=0
t0=0
ts=0
def server_send(new_socket, gp, gv):
    global count, t0,ts
    while True:
        # p=copy.deepcopy(gp) # 必须copy，否则传递的只是个指针
        # v=copy.deepcopy(gv)
        try: 
            t1=time()
            p=np.array(gp, dtype='float16')
            v=np.array(gv, dtype='float16')
            v1=np.concatenate((v, np.zeros((len(v),1), dtype='float16')), axis=1) # 使v与p同维
            data=np.concatenate((p,v1), axis=0)
            print(data[0][0])
            new_socket.send(data.tobytes())
            count+=1
            sleep(0.01)
            t2=time()
            ts+=t2-t1
            # print(f'count:{count},avg time gap:{ts/count}')
        except socket.error or ConnectionResetError as e:
            print(e)
            break

def run(p, v):
    global t0
    pool=ThreadPoolExecutor(max_workers=3)

    server_addr=('192.168.3.5', 7788)
    server_socket=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server_socket.bind(server_addr)
    server_socket.listen()
    print(f'tcp server started at {server_addr}')
    while True:
        new_socket, addr=server_socket.accept()
        print(f'received req from {addr}')
        t0=time()
        pool.submit(server_send, new_socket, p, v)

if __name__=='__main__':
    
    run(poses(), speeds())